/*
  Sept 2013

  bgc32 Rev -

  Copyright (c) 2013 John Ihlein.  All rights reserved.

  Open Source STM32 Based Brushless Gimbal Controller Software

  Includes code and/or ideas from:

  1)AeroQuad
  2)BaseFlight
  3)CH Robotics
  4)MultiWii
  5)S.O.H. Madgwick
  6)UAVX

  Designed to run on the EvvGC Brushless Gimbal Controller Board

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

///////////////////////////////////////////////////////////////////////////////

#include "board.h"
#include "stm32f10x.h"
///////////////////////////////////////////////////////////////////////////////

// TIM8 Roll
// PC6, PC7, PC8 used for Roll,  TIM_OCPolarity_High
// PA7, PB0, PB1 used for RollN, TIM_OCPolarity_High

#define ROLL_A_GPIO   GPIOA
#define ROLL_A_PIN    GPIO_Pin_6
#define ROLL_B_GPIO   GPIOA
#define ROLL_B_PIN    GPIO_Pin_3
#define ROLL_C_GPIO   GPIOA
#define ROLL_C_PIN    GPIO_Pin_2

// #define ROLL_AN_GPIO  GPIOA
// #define ROLL_AN_PIN   GPIO_Pin_7
// #define ROLL_BN_GPIO  GPIOB
// #define ROLL_BN_PIN   GPIO_Pin_0
// #define ROLL_CN_GPIO  GPIOB
// #define ROLL_CN_PIN   GPIO_Pin_1

// TIM1 Pitch
// PA8,  PA9,  PA10 used for Pitch,  TIM_OCPolarity_High
// PB13, PB14, PB15 used for PitchN, TIM_OCPolarity_High

#define PITCH_A_GPIO    GPIOB
#define PITCH_A_PIN     GPIO_Pin_1
#define PITCH_B_GPIO    GPIOB
#define PITCH_B_PIN     GPIO_Pin_0
#define PITCH_C_GPIO    GPIOA
#define PITCH_C_PIN     GPIO_Pin_7

// #define PITCH_AN_GPIO   GPIOB
// #define PITCH_AN_PIN    GPIO_Pin_13
// #define PITCH_BN_GPIO   GPIOB
// #define PITCH_BN_PIN    GPIO_Pin_14
// #define PITCH_CN_GPIO   GPIOB
// #define PITCH_CN_PIN    GPIO_Pin_15

// TIM5 Yaw
// PA0, PA1, PA2 used for Yaw,  TIM_OCPolarity_High

#define YAW_A_GPIO     GPIOB
#define YAW_A_PIN      GPIO_Pin_9
#define YAW_B_GPIO     GPIOA
#define YAW_B_PIN      GPIO_Pin_1
#define YAW_C_GPIO     GPIOB
#define YAW_C_PIN      GPIO_Pin_8

// TIM4 YawN
// PB6, PB7, PB8 used for YawN, TIM_OCPolarity_Low

// #define YAW_AN_GPIO    GPIOB
// #define YAW_AN_PIN     GPIO_Pin_6
// #define YAW_BN_GPIO    GPIOB
// #define YAW_BN_PIN     GPIO_Pin_7
// #define YAW_CN_GPIO    GPIOB
// #define YAW_CN_PIN     GPIO_Pin_8

///////////////////////////////////////
// 1200 : 15khz
// 600 : 30khz      roll/pitch电机有异响
// 300 : 60khz      pitch电机还有异响
// 240 : 75khz      pitch电机还有异响
// 200 : 90khz      不工作
#define PWM_PERIOD 900

#define MAX_CNT (PWM_PERIOD * 8 / 10)

#define BB_PERIPH_ADDR(addr, bit) ((vu32*)(PERIPH_BB_BASE + ((void*)(addr)-(void*)PERIPH_BASE) * 32 + (bit) * 4))

///////////////////////////////////////

static int pwmMotorDriverInitDone = false;

int timer1timer8deadTimeRegister = 200; // this is not just a delay value, check CPU reference manual for TIMx_BDTR DTG bit 0-7
int timer4timer5deadTimeDelay    = 80;  // in 18MHz ticks

static int rollPhase[3], pitchPhase[3], yawPhase[3];

int maxCnt[NUMAXIS];
int minCnt[NUMAXIS];
int irqCnt[NUMAXIS];

uint8_t oddEvenFrame = 0;

///////////////////////////////////////////////////////////////////////////////
//  Counter Updates
///////////////////////////////////////////////////////////////////////////////

/*inline */void updateCounter(uint8_t channel, int value)
{
    irqCnt[channel]++;

    if (value > maxCnt[channel])
    {
        maxCnt[channel] = value;
    }

    if (value < minCnt[channel])
    {
        minCnt[channel] = value;
    }
}

///////////////////////////////////////////////////////////////////////////////
//  IRQ Setup
///////////////////////////////////////////////////////////////////////////////

static void setupPWMIrq(uint8_t irq)
{
    NVIC_InitTypeDef NVIC_InitStructure;

    NVIC_InitStructure.NVIC_IRQChannel                   = irq;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;       //Preemption Priority
    NVIC_InitStructure.NVIC_IRQChannelSubPriority        = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd                = ENABLE;

    NVIC_Init(&NVIC_InitStructure);
}

///////////////////////////////////////////////////////////////////////////////
//  TIM8 IRQ Handler (ROLL)
///////////////////////////////////////////////////////////////////////////////
void TIM2_IRQHandler(void) // ROLL axis
{	
		unsigned short cnt ;
    if (TIM2->SR & TIM_SR_UIF) // if UIF flag is set
    {
        TIM2->SR &= ~TIM_SR_UIF; // clear UIF flag
        // cliPrintF("  Enter TIM2 \n ");

        __disable_irq_nested();
        cnt = TIM2->CNT;
        updateCounter(ROLL, cnt);

        if (cnt < MAX_CNT)
        {
            // make sure there is enough time to make all changes
            if (eepromConfig.rollEnabled)
            {
                TIM3->CCR1 = rollPhase[0];
                TIM2->CCR4 = rollPhase[1];
                TIM2->CCR3 = rollPhase[2];
            }
            else
            {
                TIM3->CCR1 = 0;
                TIM2->CCR4 = 0;
                TIM2->CCR3 = 0;
            }

            TIM2->DIER &= ~TIM_DIER_UIE;  // disable update interrupt
        }

        __enable_irq_nested();
    }
    // cliPrintF(" Out TIM2 \n");

}
///////////////////////////////////////////////////////////////////////////////
//  TIM1 IRQ Handler (PITCH)
///////////////////////////////////////////////////////////////////////////////
void TIM3_IRQHandler(void) // PITCH axis
{	
		unsigned short cnt ;
    if (TIM3->SR & TIM_SR_UIF) // if UIF flag is set
    {
        TIM3->SR &= ~TIM_SR_UIF; // clear UIF flag
        // cliPrintF(" Enter TIM3 \n");

        __disable_irq_nested();
        cnt = TIM3->CNT;
        updateCounter(PITCH, cnt);

        if (cnt < MAX_CNT)
        {
            // make sure there is enough time to make all changes
            if (eepromConfig.pitchEnabled)
            {
                TIM_SetCompare4(TIM3,pitchPhase[0]);
                TIM_SetCompare3(TIM3,pitchPhase[1]);
                TIM_SetCompare2(TIM3,pitchPhase[2]);
                // TIM3->CCR4 = pitchPhase[0];
                // TIM3->CCR3 = pitchPhase[1];
                // TIM3->CCR2 = pitchPhase[2];
            }
            else
            {
                TIM3->CCR4 = 0;
                TIM3->CCR3 = 0;
                TIM3->CCR2 = 0;

            }

            TIM3->DIER &= ~TIM_DIER_UIE;  // disable update interrupt
        }

        __enable_irq_nested();
    }
    // cliPrintF(" Out TIM3 \n");

}
///////////////////////////////////////////////////////////////////////////////
//  TIM5 IRQ Handler (YAW)
///////////////////////////////////////////////////////////////////////////////
void TIM4_IRQHandler(void) // yaw axis
{	
    
		unsigned short cnt ;
    
    if (TIM4->SR & TIM_SR_UIF) // if UIF flag is set
    {
        TIM4->SR &= ~TIM_SR_UIF; // clear UIF flag
        // cliPrintF(" Enter TIM4 \n");
        __disable_irq_nested();
        cnt = TIM4->CNT;
        updateCounter(YAW, cnt);

        if (cnt < MAX_CNT)
        {
            // make sure there is enough time to make all changes
            if (eepromConfig.yawEnabled)
            {
                TIM_SetCompare4(TIM4,yawPhase[0]);
                TIM_SetCompare2(TIM2,yawPhase[1]);
                TIM_SetCompare3(TIM4,yawPhase[2]);
                // TIM4->CCR4 = yawPhase[0];
                // TIM2->CCR2 = yawPhase[1];
                // TIM4->CCR3 = yawPhase[2];
            }
            else
            {
                TIM4->CCR4 = 0;
                TIM2->CCR2 = 0;
                TIM4->CCR3 = 0;
            }

            TIM4->DIER &= ~TIM_DIER_UIE;  // disable update interrupt
        }

        __enable_irq_nested();
    }
    // cliPrintF(" Out TIM4 \n");
}
///////////////////////////////////////////////////////////////////////////////
//  Timer Channel Configuration
///////////////////////////////////////////////////////////////////////////////

static void timerChannelConfig(TIM_TypeDef *tim, TIM_OCInitTypeDef *OCInitStructure)
{
    TIM_OC1Init(tim, OCInitStructure);
    TIM_OC2Init(tim, OCInitStructure);
    TIM_OC3Init(tim, OCInitStructure);
    TIM_OC4Init(tim, OCInitStructure);

    TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
    TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
    TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
    TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
}

///////////////////////////////////////////////////////////////////////////////
//  General Timer Configuration
///////////////////////////////////////////////////////////////////////////////

static void timerPWMgeneralConfig(TIM_TypeDef *tim, int polarity)
{
    TIM_TimeBaseInitTypeDef     TIM_TimeBaseInitStructure;
    TIM_OCInitTypeDef           TIM_OCInitStructure;

    TIM_TimeBaseInitStructure.TIM_Prescaler     = (4 - 1);                 // 72 Mhz / (3 + 1) = 18 MHz
    TIM_TimeBaseInitStructure.TIM_CounterMode   = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_Period        = PWM_PERIOD - 1;          // 18 Mhz / 1000 = 18 kHz
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;

    TIM_TimeBaseInit(tim, &TIM_TimeBaseInitStructure);

    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1 ;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse       = 0;
    TIM_OCInitStructure.TIM_OCPolarity  = polarity;

    timerChannelConfig(tim, &TIM_OCInitStructure);
}

///////////////////////////////////////////////////////////////////////////////
//  Set PWM Via Table Lookup
///////////////////////////////////////////////////////////////////////////////

void setPWMFastTable(int *pwm, float angle, float power)
{
	int iPower;
	int angleInt ;
    if (testPhase >= 0)
    {
        angle = testPhase;
    }

    angleInt = (int)round(angle / M_TWOPI * SINARRAYSIZE);

    angleInt = angleInt % SINARRAYSIZE;

    if (angleInt < 0)
    {
        angleInt = SINARRAYSIZE + angleInt;
    }

    //int iPower = 5 * (int)power;
    // cliPrintF(" rollPhase[0] %d ", (int)(power));
     iPower = (int)((PWM_PERIOD / 2)  * power / 100);

    // cliPrintF(" rollPhase[0] %d ", (int)(iPower));

    pwm[0] = (sinDataI16[ angleInt                               % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
    pwm[1] = (sinDataI16[(angleInt +  1 * SINARRAYSIZE / 3)      % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
    pwm[2] = (sinDataI16[(angleInt + (2 * SINARRAYSIZE + 1) / 3) % SINARRAYSIZE] * iPower + SINARRAYSCALE / 2) / SINARRAYSCALE + PWM_PERIOD / 2;
}

///////////////////////////////////////////////////////////////////////////////
//  Set PWM
///////////////////////////////////////////////////////////////////////////////

void setPWM(int *pwm, float angle, float power)
{
    setPWMFastTable(pwm, angle, power);
}

///////////////////////////////////////////////////////////////////////////////
//  Set PWM Data
///////////////////////////////////////////////////////////////////////////////

void setPWMData(int *target, int *pwm)
{
    __disable_irq_nested();
    target[0] = pwm[0];
    target[1] = pwm[1];
    target[2] = pwm[2];
    __enable_irq_nested();
}

///////////////////////////////////////////////////////////////////////////////
//  Limit Yaw PWM
///////////////////////////////////////////////////////////////////////////////

void limitYawPWM(int *pwm)
{
    int i;
    int maxVal = PWM_PERIOD - 2 * timer4timer5deadTimeDelay;

    for (i = 0; i < 3; i++)
    {
        pwm[i] -= timer4timer5deadTimeDelay;

        if (pwm[i] >= maxVal)
        {
            pwm[i] = maxVal;
        }

        if (pwm[i] < timer4timer5deadTimeDelay)
        {
            pwm[i] = 0;
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
//  Activate Interrupt
///////////////////////////////////////////////////////////////////////////////

void activateIRQ(TIM_TypeDef *tim)
{
    __disable_irq_nested();
    tim->SR &= ~TIM_SR_UIF;   // clear UIF flag
    tim->DIER = TIM_DIER_UIE; // Enable update interrupt
    __enable_irq_nested();
}

///////////////////////////////////////////////////////////////////////////////
//  Set Roll Axis PWM
///////////////////////////////////////////////////////////////////////////////

void setRollMotor(float phi, int power)
{
    int pwm[3];

    // cliPrintF(" roll %d ", (int)(phi*100));
    setPWM(pwm, phi, power);
    
//    cliPrintF(" rollPhase[0] %d ", (int)(pwm[0]));
//    cliPrintF(" rollPhase[1] %d ", (int)(pwm[1]));
//    cliPrintF(" rollPhase[2] %d \n", (int)(pwm[2]));
    setPWMData(rollPhase, pwm);
    // TIM_SetCompare1(TIM3,rollPhase[0]);
    // TIM_SetCompare4(TIM2,rollPhase[1]);
    // TIM_SetCompare3(TIM2,rollPhase[2]);
    activateIRQ(TIM3);
    activateIRQ(TIM2);
}

///////////////////////////////////////////////////////////////////////////////
//  Set Pitch Axis PWM
///////////////////////////////////////////////////////////////////////////////

void setPitchMotor(float theta, int power)
{
    int pwm[3];

    // cliPrintF(" pitch %d ", (int)(theta*100));
    setPWM(pwm, theta, power);
    
//    cliPrintF(" pitchPhase[0] %d ", (int)(pwm[0]));
//    cliPrintF(" pitchPhase[1] %d ", (int)(pwm[1]));
//    cliPrintF(" pitchPhase[2] %d \n", (int)(pwm[2]));
    setPWMData(pitchPhase, pwm);
    // TIM_SetCompare4(TIM3,pitchPhase[0]);
    // TIM_SetCompare3(TIM3,pitchPhase[1]);
    // TIM_SetCompare2(TIM3,pitchPhase[2]);
    activateIRQ(TIM3);
}

///////////////////////////////////////////////////////////////////////////////
//  Set Yaw Axis PWM
///////////////////////////////////////////////////////////////////////////////

void setYawMotor(float psi, int power)
{
    int pwm[3];

    // cliPrintF(" yaw %d \n", (int)(psi*100));
    setPWM(pwm, psi, power);

//    cliPrintF(" yawPhase[0] %d ", (int)(pwm[0]));
//    cliPrintF(" yawPhase[1] %d ", (int)(pwm[1]));
//    cliPrintF(" yawPhase[2] %d \n", (int)(pwm[2]));
    limitYawPWM(pwm);
    setPWMData(yawPhase, pwm);
    // TIM_SetCompare4(TIM4,yawPhase[0]);
    // TIM_SetCompare2(TIM2,yawPhase[1]);
    // TIM_SetCompare3(TIM4,yawPhase[2]);
    activateIRQ(TIM2);
    activateIRQ(TIM4);
}

///////////////////////////////////////////////////////////////////////////////
//  Force Motor Update
///////////////////////////////////////////////////////////////////////////////

void forceMotorUpdate(void)
{
    activateIRQ(TIM2);
    activateIRQ(TIM4);
    activateIRQ(TIM3);
}

///////////////////////////////////////////////////////////////////////////////
//  Initialize PWM Motor Drivers
///////////////////////////////////////////////////////////////////////////////

void pwmMotorDriverInit(void)
{
	GPIO_InitTypeDef         GPIO_InitStructure;
	
    if (pwmMotorDriverInitDone)
    {
        forceMotorUpdate();
        // make sure this init function is not called twice
        return;
    }
    ///////////////////////////////////

    // Roll PWM Timer Initialization here

    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;

    GPIO_InitStructure.GPIO_Pin   = ROLL_A_PIN | ROLL_B_PIN | ROLL_C_PIN | PITCH_C_PIN | YAW_B_PIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin   = PITCH_A_PIN | PITCH_B_PIN | YAW_A_PIN | YAW_C_PIN;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    irqCnt[YAW] = 0;
    maxCnt[YAW] = 0;
    minCnt[YAW] = PWM_PERIOD + 1;

    irqCnt[ROLL] = 0;
    maxCnt[ROLL] = 0;
    minCnt[ROLL] = PWM_PERIOD + 1;

    irqCnt[PITCH] = 0;
    maxCnt[PITCH] = 0;
    minCnt[PITCH] = PWM_PERIOD + 1;

    timerPWMgeneralConfig(TIM2, TIM_OCPolarity_High);
    timerPWMgeneralConfig(TIM3, TIM_OCPolarity_High);
    timerPWMgeneralConfig(TIM4, TIM_OCPolarity_High);

    // TIM3->CNT = timer4timer5deadTimeDelay + 5 + PWM_PERIOD * 2 / 3;  // 751

    setupPWMIrq(TIM3_IRQn);
    setupPWMIrq(TIM2_IRQn);
    setupPWMIrq(TIM4_IRQn);

    __disable_irq_nested();
    {
        TIM_ARRPreloadConfig(TIM4,ENABLE);
        TIM_ARRPreloadConfig(TIM3,ENABLE);
        TIM_ARRPreloadConfig(TIM2,ENABLE);
        TIM_Cmd(TIM3, ENABLE);
        TIM_Cmd(TIM2, ENABLE);
        TIM_Cmd(TIM4, ENABLE);
    }
    __enable_irq_nested();

    pwmMotorDriverInitDone = true;

    ///////////////////////////////////
    // Pitch PWM Timer Initialization here

    // GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
    // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;

    // GPIO_InitStructure.GPIO_Pin   = PITCH_C_PIN;
    // GPIO_Init(GPIOA, &GPIO_InitStructure);

    // GPIO_InitStructure.GPIO_Pin   = PITCH_A_PIN | PITCH_B_PIN ;
    // GPIO_Init(GPIOB, &GPIO_InitStructure);

    // irqCnt[PITCH] = 0;
    // maxCnt[PITCH] = 0;
    // minCnt[PITCH] = PWM_PERIOD + 1;

    // timerPWMadvancedConfig(TIM1);
    // timerPWMgeneralConfig(TIM2, TIM_OCPolarity_Low);

    // TIM2->CNT = timer4timer5deadTimeDelay + 3 + PWM_PERIOD / 3;  // 416

    // setupPWMIrq(TIM2_IRQn);

    // __disable_irq_nested();
    {
//        vu32 *tim1Enable = BB_PERIPH_ADDR(&(TIM1->CR1), 0);
//        *tim1Enable = 1;

        // TIM_Cmd(TIM2, ENABLE);
        // TIM_CtrlPWMOutputs(TIM2, ENABLE);
    }
    // __enable_irq_nested();


    ///////////////////////////////////
    // Yaw PWM Timers Initialization here

//     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
//     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;

//     GPIO_InitStructure.GPIO_Pin   = YAW_B_PIN;
//     GPIO_Init(GPIOA, &GPIO_InitStructure);

//     GPIO_InitStructure.GPIO_Pin   = YAW_A_PIN | YAW_C_PIN;
//     GPIO_Init(GPIOB, &GPIO_InitStructure);

//     irqCnt[YAW] = 0;
//     maxCnt[YAW] = 0;
//     minCnt[YAW] = PWM_PERIOD + 1;

//     // timerPWMgeneralConfig(TIM5, TIM_OCPolarity_High);
//     timerPWMgeneralConfig(TIM4, TIM_OCPolarity_Low);

//     // setupPWMIrq(TIM4_IRQn);

//     __disable_irq_nested();
//     {
// //        vu32 *tim5Enable = BB_PERIPH_ADDR(&(TIM5->CR1), 0);
// //        vu32 *tim4Enable = BB_PERIPH_ADDR(&(TIM4->CR1), 0);

//         // TIM4->CNT = timer4timer5deadTimeDelay;
// //        *tim5Enable = 1;
// //        *tim4Enable = 1;
//         TIM_Cmd(TIM4, ENABLE);
//         // TIM_Cmd(TIM5, ENABLE);
			
//         // TIM_CtrlPWMOutputs(TIM5, ENABLE);
//         // TIM_CtrlPWMOutputs(TIM4, ENABLE);
//     }
//     __enable_irq_nested();

    ///////////////////////////////////

}

///////////////////////////////////////////////////////////////////////////////
